package butines.game2.pathfinder;

import java.awt.Color;
import java.awt.Graphics2D;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.LinkedList;
import java.util.List;

import butines.common.math.Vector2D;

public class Pathfinder {

    private Map map;

    private float[] gCosts;
    private float[] fCosts;
    private int[] navigated;
    
    private List<Integer> closedset;
    private List<Integer> openset;
    private LinkedList<Integer> path;
    
    private boolean avoidCorners = true;
    
    private final Comparator<Integer> order = new Comparator<Integer>() {

        @Override
        public int compare(Integer a, Integer b) {
            if (fCosts[a] < fCosts[b]) {
                return -1;
            } else if (fCosts[a] > fCosts[b]) {
                return 1;
            }
            return 0;
        }

    };
    
    
    public Pathfinder(Map map) {
        this.map = map;
        gCosts = new float[map.getNumCells()];
        fCosts = new float[map.getNumCells()];
        navigated = new int[map.getNumCells()];
        closedset = new ArrayList<Integer>();
        openset = new ArrayList<Integer>();
        path = new LinkedList<Integer>();
    }

    public List<Integer> getPath() {
        return path;
    }
    
    private float distance(int from, int to) {
        Vector2D a = map.getPosition(from);
        Vector2D b = map.getPosition(to);
        return a.distance(b);
    }

    private List<Integer> reconstructPath(int currentNode) {
        path.clear();        
        path.addFirst(currentNode);
        
        currentNode = navigated[currentNode];
        while (currentNode != -1) {
            path.addFirst(currentNode);
            currentNode = navigated[currentNode];
        }
        
        return path;
    }
    
    private boolean isValidLocation(int current, int neighbor) {
        int sx = current % map.getNumCellsX();
        int sy = current / map.getNumCellsX();
        
        int x = neighbor % map.getNumCellsX();
        int y = neighbor / map.getNumCellsX();
        
        return isValidLocation(sx, sy, x, y);
    }
    
    private boolean isValidLocation(int sx, int sy, int x, int y) {
        boolean valid = map.isValidCell(x, y) && map.isPassableGrid(x, y);
        
        if (valid && avoidCorners) {
            if (x-sx != 0 && y-sy != 0) {
                valid = map.isPassableGrid(x, sy) && map.isPassableGrid(sx, y);
            }
        }
        
        return valid;
    }
    
    public boolean search(int startx, int starty, int goalx, int goaly) {
        int start = starty * map.getNumCellsX() + startx;
        int goal = goaly * map.getNumCellsX() + goalx;
        return search(start, goal);
    }
    
    public boolean search(int start, int goal) {
        
        closedset.clear();
        openset.clear();

        Arrays.fill(gCosts, 0);
        Arrays.fill(fCosts, 0);
        Arrays.fill(navigated, -1);
        
        gCosts[start] = 0;
        fCosts[start] = gCosts[start] + distance(start, goal);

        openset.add(start);
        
        while (!openset.isEmpty()) {
        	
        	Collections.sort(openset, order);
            
            int current = openset.remove(0);
            
            if (current == goal) {
                reconstructPath(goal);
                return true;
            }
            
            closedset.add(current);

            for (int neighbor : map.getNeighbours(current)) {
                
                if (closedset.contains(neighbor)) {
                    continue;
                }
                
                if (!isValidLocation(current, neighbor)) {
                    continue;
                }
                
                float gCost = gCosts[current] + distance(current, neighbor);
                
                if (!openset.contains(neighbor) || gCost < gCosts[neighbor]) {
                    navigated[neighbor] = current;
                    gCosts[neighbor] = gCost;
                    fCosts[neighbor] = gCosts[neighbor] + distance(neighbor, goal);
                    if (!openset.contains(neighbor)) {
                        openset.add(neighbor);
                    }
                }
                
            }
            
        }
        
        return false;
        
    }
       
    public void debug(Graphics2D g2d, boolean drawCloseset) {
        if (path.isEmpty()) {
            return;
        }
       
        
        if (drawCloseset) {
            g2d.setColor(Color.ORANGE);
            for (int idx : closedset) {
                Vector2D n1 = map.getPosition(idx);
                int x = Math.round(n1.x) - 3;
                int y = Math.round(n1.y) - 3;
                g2d.fillOval(x, y, 5, 5);
             }
        }
        
        g2d.setColor(Color.RED);
        int from = -1;
        for (int idx : path) {
           Vector2D n1 = map.getPosition(idx);
           
           if (from != -1) {
               Vector2D n2 = map.getPosition(from);
               int x1 = Math.round(n1.x);
               int y1 = Math.round(n1.y);
               int x2 = Math.round(n2.x);
               int y2 = Math.round(n2.y);
               g2d.drawLine(x1, y1, x2, y2);
           }
           
           int x = Math.round(n1.x) - 3;
           int y = Math.round(n1.y) - 3;
           g2d.fillOval(x, y, 5, 5);
           from = idx;
        }
        from = -1;
        
    }
    
}
